////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "QuadTreeNode.hpp"

#include <SFML/OpenGL.hpp>

#include "QuadTree.hpp"

#include <SmegTools/Debug/DebugNew.hpp>

namespace smeg {
    namespace math {
        
////////////////////////////////////////////////////////////
// Defines
////////////////////////////////////////////////////////////
#define MAX_OCCUPANTS 6
#define MAX_LEVELS 8
        
////////////////////////////////////////////////////////////
/// Default constructor
////////////////////////////////////////////////////////////
QuadTreeNode::QuadTreeNode() :
    m_Region(),
    m_pTree( NULL ),
    m_pParentNode( NULL ),
    m_uLevel( 0 ),
    m_uNumOccupantsBelow( 0 ),
    m_Occupants(),
    m_bHasChildren( false ),
    m_Children()
{
}

////////////////////////////////////////////////////////////
/// Constructor
////////////////////////////////////////////////////////////
QuadTreeNode::QuadTreeNode( const math::BoundingBox& _Region, QuadTree* _pTree, QuadTreeNode* _pParent, const unsigned _uLevel ) :
    m_Region( _Region ),
    m_pTree( _pTree ),
    m_pParentNode( _pParent ),
    m_uLevel( 0 ),
    m_uNumOccupantsBelow( 0 ),
    m_Occupants(),
    m_bHasChildren( false ),
    m_Children()
{
    Create( _Region, _pTree, _pParent, _uLevel );
}
    
////////////////////////////////////////////////////////////
/// Destructor
////////////////////////////////////////////////////////////
QuadTreeNode::~QuadTreeNode()
{
    DestroyChildren();
}

////////////////////////////////////////////////////////////
/// Create the node
////////////////////////////////////////////////////////////
void QuadTreeNode::Create( const math::BoundingBox& _Region, QuadTree* _pTree, QuadTreeNode* _pParent, const unsigned _uLevel ) {
    m_Region = _Region;
    m_pTree = _pTree;
    m_pParentNode = _pParent;
    m_uLevel = _uLevel;
    
    m_uNumOccupantsBelow = 0;
    m_bHasChildren = false;
}

////////////////////////////////////////////////////////////
/// Try to add an occupant to this node
////////////////////////////////////////////////////////////
bool QuadTreeNode::Add( core::BaseObject* _pNewOccupant ) {
    if ( m_Region.ContainsUnknownBBPtr( _pNewOccupant->GetBoundingVolume() ) ) {
        ++m_uNumOccupantsBelow;

        // OK the occupant fit the node
        if ( m_bHasChildren ) {
            if ( AddToChildren( _pNewOccupant ) )
                return true; // OK Occupant added to a child, nothing more to do
        } else {
            // Check if we need to split the node or if we can add the occupant to this level
            if ( m_Occupants.size() >= MAX_OCCUPANTS && m_uLevel < MAX_LEVELS ) {
                Partition();

                if ( AddToChildren( _pNewOccupant ) )
                    return true;
            }
        }

        // Add To This level because we couldn't put it elsewhere
        AddToThisLevel( _pNewOccupant );

        return true;
    }

    return false;
}

////////////////////////////////////////////////////////////
/// Update the occupation of this node with the given occupant
////////////////////////////////////////////////////////////
void QuadTreeNode::Update( core::BaseObject* _pOccupant ) {
    // Node that will be merged
    QuadTreeNode* lastNodeToMerge = NULL;

    // If no occupants or the node of the occupant isn't this, we can't update it
    if ( m_Occupants.empty() || this != _pOccupant->GetNode() ) {
        return;
    }

    // Remove from the node (may be added again later)
    m_Occupants.erase( _pOccupant );

    // Find the lowest node from this that can contain the occupant
    QuadTreeNode* pCurrentNode = this;
    while ( pCurrentNode ) {
        --pCurrentNode->m_uNumOccupantsBelow;

        if ( pCurrentNode->m_Region.ContainsUnknownBBPtr( _pOccupant->GetBoundingVolume() ) ) {
            break;
        } else if ( pCurrentNode->m_uNumOccupantsBelow <= MAX_OCCUPANTS ) {
            // If the number of occupants below is less or equal to the maximum of occupants a node can carry, merge this node
            lastNodeToMerge = pCurrentNode;
        }

        pCurrentNode = pCurrentNode->m_pParentNode;
    }

    // If we found a spot, add it
    if ( pCurrentNode ) {
        pCurrentNode->Add( _pOccupant );
    } else {
        // If not need to add outside
        m_pTree->AddOutside( _pOccupant );
    }

    if ( lastNodeToMerge ) {
        lastNodeToMerge->Merge();
    }
}

////////////////////////////////////////////////////////////
/// Remove the occupation from this node
////////////////////////////////////////////////////////////
void QuadTreeNode::Remove( core::BaseObject* _pOccupant ) {
    // Node that will be merged
    QuadTreeNode* lastNodeToMerge = NULL;

    // If no occupants or the node of the occupant isn't this, we can't remove it
    if ( m_Occupants.empty() || this != _pOccupant->GetNode() ) {
        return;
    }

    // Remove from this node
    m_Occupants.erase( _pOccupant );

    // Propagate the removal upward
    QuadTreeNode* pCurrentNode = this;
    while ( pCurrentNode ) {
        --pCurrentNode->m_uNumOccupantsBelow;

        if ( pCurrentNode->m_uNumOccupantsBelow <= MAX_OCCUPANTS ) {
            // If the number of occupants below is less or equal to the maximum of occupants a node can carry, merge this node
            lastNodeToMerge = pCurrentNode;
        }

        pCurrentNode = pCurrentNode->m_pParentNode;
    }

    if ( lastNodeToMerge ) {
        lastNodeToMerge->Merge();
    }
}

////////////////////////////////////////////////////////////
/// Retrieve the occupants in this node and its children
////////////////////////////////////////////////////////////
void QuadTreeNode::GetAllOccupantsBelow( std::vector<core::BaseObject*>& _Occupants, BoundingVolume::eBoundingVolume _eType ) {
    QuadTreeNode* pCurrentNode = this;
    std::vector< QuadTreeNode* > nodeToParse;

    nodeToParse.push_back( this );
    while ( !nodeToParse.empty() ) {
        pCurrentNode = nodeToParse.back();
        nodeToParse.pop_back();

        // Add the occupants
        for ( td_OccupantsSetIt it = pCurrentNode->m_Occupants.begin(); it != pCurrentNode->m_Occupants.end(); ++it ) {
            if ( (*it)->HasBoundingVolume( _eType ) ) {
                _Occupants.push_back( *it );
            }
        }

        // Get the children if any
        if ( pCurrentNode->m_bHasChildren ) {
            for ( unsigned i = 0; i < 2; ++i ) {
                for ( unsigned j = 0; j < 2; ++j ) {
                    nodeToParse.push_back( pCurrentNode->m_Children[i][j] );
                }
            }
        }
    }
}

////////////////////////////////////////////////////////////
/// Destroy all children
////////////////////////////////////////////////////////////
void QuadTreeNode::DestroyChildren() {
    if ( m_bHasChildren ) {
        for ( unsigned i = 0; i < m_Children.size(); ++i ) {
            for ( unsigned j = 0; j < m_Children[i].size(); ++j ) {
                delete m_Children[i][j];
            }
        }

        m_Children.clear();
        m_bHasChildren = false;
    }
}

////////////////////////////////////////////////////////////
/// Partition the node into 4 children nodes
////////////////////////////////////////////////////////////
void QuadTreeNode::Partition() {
    // Do not do a partition if we already have one
    if ( m_bHasChildren ) {
        return;
    }
    
    const sf::Vector2f halfDims( m_Region.GetHalfSize() ), newHalfDims( halfDims * 0.5f );
    const sf::Vector2f lowerBound( m_Region.GetLowerBound() );
    const unsigned uNextLevel = m_uLevel + 1;

    sf::Vector2f baseNewCenter( lowerBound + newHalfDims ), newCenter;
    math::BoundingBox childrenAABB;

    //    +---------------+---------------+
    //    |               |               |
    //    |      0,1      |      1,1      |
    //    |               |               |
    //    +---------------+---------------+
    //    |               |               |
    //    |      0,0      |      1,0      |
    //    |               |               |
    //    +---------------+---------------+
    //
    //    baseNewCenter is at point 1

    m_Children.resize( 2 );
    for ( unsigned i = 0; i < 2; ++i ) {
        m_Children[i].resize( 2 );
        for ( unsigned j = 0; j < 2; ++j ) {
            newCenter.x = baseNewCenter.x + halfDims.x * i;
            newCenter.y = baseNewCenter.y + halfDims.y * j;

            childrenAABB.Setup( newCenter, halfDims );

            m_Children[i][j] = new QuadTreeNode( childrenAABB, m_pTree, this, uNextLevel );
        }
    }

    m_bHasChildren = true;

    // Partition is done, put all the occupants in the newly created children (when possible)
    core::BaseObject* pCurrentOccupant;
    td_OccupantsSet oldNodeOccupants( m_Occupants );
    m_Occupants.clear();

    for ( td_OccupantsSetIt it = oldNodeOccupants.begin(); it != oldNodeOccupants.end(); ++it ) {
        // If we successfully add the occupant to the child, remove it from the old node list and go to the next
        // Otherwise put it in this node
        pCurrentOccupant = *it;

        if ( !AddToChildren( pCurrentOccupant ) ) {
            AddToThisLevel( pCurrentOccupant );
        }
    }
}

////////////////////////////////////////////////////////////
/// Merge the children into this node
////////////////////////////////////////////////////////////
void QuadTreeNode::Merge() {
    if ( m_bHasChildren ) {
        std::vector< core::BaseObject* > allOccupants;
        GetAllOccupantsBelow( allOccupants );
        // Set this as the QuadTreeNode
        for ( unsigned i = 0; i != m_uNumOccupantsBelow; ++i ) {
            allOccupants[i]->SetNode( this );
            m_Occupants.insert( allOccupants[i] );
        }
        // Destroy the children
        DestroyChildren();
    }
}

////////////////////////////////////////////////////////////
/// Add an occupant to this level
////////////////////////////////////////////////////////////
void QuadTreeNode::AddToThisLevel( core::BaseObject* _pNewOccupant ) {
    _pNewOccupant->SetTree( m_pTree );
    _pNewOccupant->SetNode( this );
    m_Occupants.insert( _pNewOccupant );
}

////////////////////////////////////////////////////////////
/// Get the node that could contains _pNewOccupant, this node is the one containing the center of _pNewOccupant
////////////////////////////////////////////////////////////
QuadTreeNode* QuadTreeNode::GetNodeContaining( core::BaseObject* _pNewOccupant ) {
    if ( m_bHasChildren ) {
        const sf::Vector2f& occupantCenter( _pNewOccupant->getPosition() );
        unsigned x = 0, y = 0;

        for ( unsigned x = 0; x < 2; ++x ) {
            for ( unsigned y = 0; y < 2; ++y ) {
                if ( m_Children[x][y]->GetRegion().Contains( occupantCenter ) ) {
                    return m_Children[x][y];
                }
            }
        }
    }

    return NULL;
}

////////////////////////////////////////////////////////////
/// Add an occupant to this node's children
////////////////////////////////////////////////////////////
bool QuadTreeNode::AddToChildren( core::BaseObject* _pNewOccupant ) {
    if ( !m_bHasChildren ) {
        return false;
    }

    QuadTreeNode* pNodeContaining = GetNodeContaining( _pNewOccupant );

    if ( pNodeContaining && pNodeContaining->Add( _pNewOccupant ) ) {
        return true;
    }
    return false;
}

#ifdef _DEBUG
void QuadTreeNode::DebugRender( sf::RenderWindow* _pRenderWindow, const sf::Color& _Color ) const {
    unsigned uNextColor = 255 * m_uLevel / MAX_LEVELS, childColor;
    m_Region.DebugRender( _pRenderWindow, sf::Color( _Color.r, _Color.g, _Color.b, 30 ), true );

    // Render children
    if ( m_bHasChildren ) {
        for ( unsigned i = 0; i < 2; ++i ) {
            for ( unsigned j = 0; j < 2; ++j ) {
                childColor = static_cast<unsigned>( 255 * ( i * 2 + j ) * 0.25f );
                m_Children[i][j]->DebugRender( _pRenderWindow, sf::Color( 255 - childColor, childColor, uNextColor ) );
            }
        }
    }
}
#endif // _DEBUG

    } // namespace light
} // namespace smeg